#include <WiFi.h>
#include <WebSocketsClient.h>
#include <ESPAsyncWebServer.h>
#include <Preferences.h>
#include <ArduinoJson.h>
#include "send.h"
#include "gain.h"

static bool g_send_as_servo_control = false;

// ====== 目标设备名：联调时改为 "body" ======
const char* TARGET_NAME = "esp32";   // 现在先对打，之后改 "body"
String WS_TARGET_ID;                 // presence/connected 时绑定

const uint32_t SEND_INTERVAL_MS = 50; // 20Hz
static uint32_t lastSend = 0;

AsyncWebServer server(80);
Preferences preferences;

String input_ssid, input_password;
String board_name = "body";
bool wifiConfigured = false;

WebSocketsClient webSocket;

// 舵机单条控制
void handleServoControl(JsonObject& obj) {
  if (obj.containsKey("c") && obj.containsKey("p")) {
    int channel = obj["c"];
    int pulse   = obj["p"];
    pulse = constrain(pulse, 0, 4095);
    Serial.printf("设置通道 %d 脉宽 %d\n", channel, pulse);
    // TODO: 真正写到 PCA9685
  }
}

// 从 onlineUsers/onlineClients 里挑出 name==TARGET_NAME 的 id
void tryPickTargetIdFromList(JsonVariant v){
  if (!v || !v.is<JsonArray>()) return;
  for (JsonObject u : v.as<JsonArray>()){
    const char* name = u["name"] | "";
    const char* id   = u["id"]   | "";
    if (!id || !*id) continue;
    if (strcmp(name, TARGET_NAME)==0){
      if (WS_TARGET_ID != id){
        WS_TARGET_ID = id;
        Serial.printf("[info] 绑定目标 %s, id=%s\n", TARGET_NAME, WS_TARGET_ID.c_str());
      }
      return;
    }
  }
  Serial.printf("[warn] 在线列表里没有名为 %s 的设备\n", TARGET_NAME);
}

// 处理消息
void handleMessage(const char* message) {
  StaticJsonDocument<2048> doc;
  DeserializationError error = deserializeJson(doc, message);
  if (error) { Serial.println("JSON解析错误"); return; }

  const char* type = doc["type"] | "";
  if (!*type) return;

  if (strcmp(type, "heartbeat") == 0) {
    StaticJsonDocument<64> pong;
    pong["type"] = "heartbeat";
    String js; serializeJson(pong, js);
    webSocket.sendTXT(js);
    return;
  }

  if (strcmp(type, "connected") == 0 || strcmp(type, "presence") == 0) {
    if (doc.containsKey("onlineClients")) tryPickTargetIdFromList(doc["onlineClients"]);
    if (doc.containsKey("onlineUsers"))   tryPickTargetIdFromList(doc["onlineUsers"]);

    // === 新增：收到 connected 后切换为发送 type=servo_control ===
    if (strcmp(type, "connected") == 0) {
      g_send_as_servo_control = true;
      Serial.println("[mode] 已切换：开始发送 type=servo_control 供 server-c 解析");
    }
    // === 新增结束 ===
    return;
  }

  if (strcmp(type, "broadcast") == 0) {
    const char* content = doc["content"] | "";
    Serial.println(String("收到广播: ") + content);
    return;
  }

  if (strcmp(type, "private") == 0) {
    const char* fromName = doc["fromName"] | "";
    const char* content  = doc["content"]  | "";
    Serial.println(String("收到私聊 - 来自 ") + fromName + ": " + content);

    // 把 content 当作 [{"c":0,"p":xxx},...] 解析并执行
    StaticJsonDocument<256> arr;
    DeserializationError e = deserializeJson(arr, content);
    if (!e && arr.is<JsonArray>()) {
      for (JsonObject servo : arr.as<JsonArray>()) handleServoControl(servo);
    }
    return;
  }

  // 兼容：如果以后服务端改成下发 type==servo_control
  if (strcmp(type, "servo_control") == 0) {
    const char* contentStr = doc["content"] | "";
    StaticJsonDocument<256> arr;
    if (!deserializeJson(arr, contentStr) && arr.is<JsonArray>()) {
      for (JsonObject servo : arr.as<JsonArray>()) handleServoControl(servo);
    }
    return;
  }

  Serial.println("未知消息类型");
}

// WebSocket 事件
void webSocketEvent(WStype_t type, uint8_t *payload, size_t length) {
  switch (type) {
    case WStype_DISCONNECTED:
      Serial.println("WebSocket 断开连接");
      g_send_as_servo_control = false;
      break;
    case WStype_CONNECTED: {
      Serial.println("WebSocket 连接成功");
      StaticJsonDocument<128> reg;
      reg["type"] = "register";
      reg["name"] = board_name.c_str();
      String js; serializeJson(reg, js);
      Serial.println("发送注册: " + js);
      webSocket.sendTXT(js);
      break;
    }
    case WStype_TEXT:
      Serial.print("收到消息: ");
      Serial.println((char*)payload);
      handleMessage((char*)payload);
      break;
    case WStype_BIN:
      Serial.println("收到二进制数据");
      break;
    default: break;
  }
}

// ====== 简单配网页面 ======
const char html_form[] PROGMEM = R"HTML(
<!DOCTYPE html><html><head><meta charset="utf-8"><title>ESP32 配网</title></head>
<body><h2>请输入 WiFi 信息</h2>
<form action="/save" method="POST">
SSID: <input type="text" name="ssid"><br><br>
密码: <input type="password" name="pass"><br><br>
<input type="submit" value="提交">
</form></body></html>
)HTML";

void handleSave(AsyncWebServerRequest *request) {
  if (request->hasParam("ssid", true)) {
    input_ssid    = request->getParam("ssid", true)->value();
    input_password= request->getParam("pass", true)->value();
    preferences.begin("wifi", false);
    preferences.putString("ssid", input_ssid);
    preferences.putString("pass", input_password);
    preferences.end();
    request->send(200, "text/plain", "配置已保存，正在尝试连接...");
    delay(1500); ESP.restart();
  } else {
    request->send(400, "text/plain", "参数错误");
  }
}

void scanNetworks() {
  Serial.println("Scanning WiFi...");
  int n = WiFi.scanNetworks();
  if (n <= 0) { Serial.println("未找到网络"); return; }
  for (int i=0;i<n;++i) Serial.printf("%d: %s (%d)\n", i+1, WiFi.SSID(i).c_str(), WiFi.RSSI(i));
}

void startAPMode() {
  preferences.begin("wifi", false);
  preferences.clear();
  preferences.end();

  WiFi.disconnect();
  WiFi.softAP("ESP_32_AP", "00000000", 1);
  IPAddress IP = WiFi.softAPIP();
  Serial.print("AP IP: "); Serial.println(IP);

  server.on("/", HTTP_GET, [](AsyncWebServerRequest *request){ request->send(200, "text/html", html_form); });
  server.on("/save", HTTP_POST, handleSave);
  server.begin();
}

void setup() {
  Serial.begin(115200);

  initPins();
  setupADC();

  // 读取配置
  preferences.begin("wifi", false);
  String saved_ssid = preferences.getString("ssid", "时间赶马");   // 你的默认
  String saved_pass = preferences.getString("pass", "Sj5688888");
  board_name        = preferences.getString("name", "body");     // ⚠ 不要再次声明
  preferences.end();

  Serial.println("读取到的 board_name: " + board_name);
  scanNetworks();

  // 连接 WiFi
  WiFi.begin(saved_ssid.c_str(), saved_pass.c_str());
  Serial.printf("正在连接 WiFi (%s)...\n", saved_ssid.c_str());
  for (int tries=0; WiFi.status()!=WL_CONNECTED && tries<20; ++tries){ delay(1000); Serial.print("."); }
  Serial.println();

  if (WiFi.status() == WL_CONNECTED) {
    Serial.print("WiFi 连接成功, IP: "); Serial.println(WiFi.localIP());
    // WebSocket
    webSocket.begin("new_human_server.qupuba.com", 9101, "/ws");
    webSocket.onEvent(webSocketEvent);
    webSocket.setReconnectInterval(5000);
  } else {
    Serial.printf("WiFi 连接失败 (%d)\n", WiFi.status());
    startAPMode();
  }
}

void loop() {
  if (WiFi.status() != WL_CONNECTED) return;
  webSocket.loop();

  uint32_t now = millis();
  if (now - lastSend >= SEND_INTERVAL_MS) {
    lastSend = now;

    // 打印 ADC
    int a0 = readChannel(0), a1 = readChannel(1), a2 = readChannel(2);
    Serial.printf("ADC0=%4d  ADC1=%4d  ADC2=%4d  ", a0, a1, a2);

    // 只有拿到目标 ID 才发送
    if (WS_TARGET_ID.length()) {
      readFirst3AndSendServos();
      int p0 = SERVO_PULSE_MIN; // 仅用于打印映射结果
      int p1 = SERVO_PULSE_MIN;
      int p2 = SERVO_PULSE_MIN;
      Serial.printf("=>  P0=%3d P1=%3d P2=%3d\n", p0, p1, p2);
      if (g_send_as_servo_control) {
        sendServoControlAsServerControl({ {0, p0}, {1, p1}, {2, p2} });
      }
      // 本地再算一次显示
      extern int adcToPulse(int, int); // from gain.h
      p0 = adcToPulse(a0,0); p1 = adcToPulse(a1,1); p2 = adcToPulse(a2,2);
      Serial.printf("=>  P0=%3d P1=%3d P2=%3d\n", p0, p1, p2);
    } else {
      Serial.println("[wait] 尚未绑定目标ID, 跳过发送");
    }
  }

  // 心跳
  static uint32_t lastHeartbeat = 0;
  if (now - lastHeartbeat > 15000) {
    webSocket.sendTXT("{\"type\":\"heartbeat\"}");
    lastHeartbeat = now;
  }
}
